#include<TELTrack.h>

#include<TCanvas.h>
#include<TH1F.h>
#include<TH2F.h>
#include<TF1.h>

TELTrack::TELTrack(Int_t RN):RunNumber(RN){

  glob = new Global(RunNumber);
  
  Path = new Char_t[200];
  sprintf(Path,"ntuple/tele/TELHits%u.root",RunNumber);

  ff = new TFile(Path);

  if(ff->IsZombie()){

    cout << "There is no " << Path << " file" << endl;
    return;

  }

  ntuple = (TTree*)ff->Get("TELHits");
  NumberOfEvents = ntuple->GetEntries();

  _useBeamConstraint = false;
  _nTelPlanes = glob->TELNumberOfPlanes + 1;
  _eBeam = glob->Energy;

  // Read position in Z (for sorting), skip layers if requested
  
  _planeSort = new int[_nTelPlanes];
  _planePosition   = new double[_nTelPlanes];
  
  // Book local geometry arrays

  _planeID         = new int[_nTelPlanes];
  _planeShiftX     = new double[_nTelPlanes];
  _planeShiftY     = new double[_nTelPlanes];
  _planeRotZ       = new double[_nTelPlanes];
  _planeThickness  = new double[_nTelPlanes];
  _planeX0         = new double[_nTelPlanes];
  _planeResolution = new double[_nTelPlanes];
  _isActive        = new bool[_nTelPlanes];
  _nActivePlanes = 0 ;

  //radaition length for silicon 9.36cm = 93600µm
  _planeX0[0] = 93600.0;
  _planeX0[1] = _planeX0[0];
  _planeX0[2] = _planeX0[0];
  _planeX0[3] = _planeX0[0];

  //thicness of telescope planes
  _planeThickness[0] = glob->TELThickness[0];
  _planeResolution[0] = glob->TELSigmaXCogX[0];
  _planePosition[0] = glob->TELOffsetZ[0];

  for(Int_t n = 2; n < _nTelPlanes; n++){
    _planeThickness[n] = glob->TELThickness[n-1];
    _planeResolution[n] = glob->TELSigmaXCogX[n-1];
    _planePosition[n] = glob->TELOffsetZ[n-1];
  }//n

  //thicnes of MAPS plane
  _planeThickness[1] = glob->DUTThickness;

  //resolution of MAPS plane 
  _planeResolution[1] = 0.0;
  
  _isActive[0]=true;
  _isActive[1]=false;
  _isActive[2]=true;
  _isActive[3]=true;

  _nActivePlanes = glob->TELNumberOfPlanes;

  // Allocate arrays for track fitting
  _planeX  = new double[_nTelPlanes];
  _planeEx = new double[_nTelPlanes];
  _planeY  = new double[_nTelPlanes];
  _planeEy = new double[_nTelPlanes];

  _planeDist = new double[_nTelPlanes];
  _planeScat = new double[_nTelPlanes];
  
  _fitX  = new double[_nTelPlanes];
  _fitEx = new double[_nTelPlanes];
  _fitY  = new double[_nTelPlanes];
  _fitEy = new double[_nTelPlanes];
  
  arrayDim = _nTelPlanes * _nTelPlanes;
  
  _fitArray = new double[arrayDim];
  _nominalFitArray = new double[arrayDim];

  _nominalError = new double[_nTelPlanes];

}

void TELTrack::FindTracks(Int_t Event){

  Float_t DigX[glob->TELNumberOfPlanes][glob->TELMaxHit];
  Float_t DigY[glob->TELNumberOfPlanes][glob->TELMaxHit];
  Float_t CogX[glob->TELNumberOfPlanes][glob->TELMaxHit];
  Float_t CogY[glob->TELNumberOfPlanes][glob->TELMaxHit];
  
  ntuple->SetBranchAddress("DigX",&DigX[0][0]);
  ntuple->SetBranchAddress("DigY",&DigY[0][0]);
  ntuple->SetBranchAddress("CogX",&CogX[0][0]);
  ntuple->SetBranchAddress("CogY",&CogY[0][0]);
  
  cout << "Event " << Event << ": " << endl;
    

  if(Event >= NumberOfEvents){

    cout << " Error - you have superpassed number of events in run " << endl; 
    return;

  }
    
  ntuple->GetEntry(Event);

  SlopeDigX.clear();
  ConstDigX.clear();
  Chi2DigX.clear();
  SlopeDigY.clear();
  ConstDigY.clear();
  Chi2DigY.clear();

  SlopeCogX.clear();
  ConstCogX.clear();
  Chi2CogX.clear();
  SlopeCogY.clear();
  ConstCogY.clear();
  Chi2CogY.clear();

  Int_t while1;
  Bool_t FitInfo;

  Float_t* PlanePosition;
  Float_t* HitPosition;
  Float_t* HitPositionErr;
  Int_t size;

  TF1* f1 = new TF1("f1","[0]*x + [1]");
  TGraphErrors* LineGraph;

  //DigX
  while1 = 0;
  while(DigX[0][while1] > 0.0 && while1 < glob->TELMaxHit){

    size = 1;
    FitInfo = false;

    for(Int_t n = 1; n < glob->TELNumberOfPlanes; n++){

      if(DigX[n][while1] > 0.0){
	FitInfo = true;
	size++;
      }

    }//n

    PlanePosition = new Float_t[size];
    HitPosition = new Float_t[size];
    HitPositionErr = new Float_t[size];

    if(FitInfo){

      size = 0;
      for(Int_t n = 0; n < glob->TELNumberOfPlanes; n++){

	if(DigX[n][while1] > 0.0){
	  PlanePosition[size] = glob->TELOffsetZ[n];
	  HitPosition[size] = DigX[n][while1];
	  HitPositionErr[size] = glob->TELStripSizeX[0]/TMath::Sqrt(12.0);
	  size++;
	}

      }//n

      LineGraph = new TGraphErrors(size,PlanePosition,HitPosition,0,HitPositionErr);
      LineGraph->Fit("f1","Q");

      SlopeDigX.push_back(f1->GetParameter(0));
      ConstDigX.push_back(f1->GetParameter(1));
      Chi2DigX.push_back(f1->GetChisquare());

      delete LineGraph;

    }

    delete [] PlanePosition;
    delete [] HitPosition;
    delete [] HitPositionErr;

    while1++;

  }//while1

  //DigY
  while1 = 0;
  while(DigY[0][while1] > 0.0 && while1 < glob->TELMaxHit){

    size = 1;
    FitInfo = false;

    for(Int_t n = 1; n < glob->TELNumberOfPlanes; n++){

      if(DigY[n][while1] > 0.0){
	FitInfo = true;
	size++;
      }

    }//n

    PlanePosition = new Float_t[size];
    HitPosition = new Float_t[size];
    HitPositionErr = new Float_t[size];

    if(FitInfo){

      size = 0;
      for(Int_t n = 0; n < glob->TELNumberOfPlanes; n++){

	if(DigY[n][while1] > 0.0){
	  PlanePosition[size] = glob->TELOffsetZ[n];
	  HitPosition[size] = DigY[n][while1];
	  HitPositionErr[size] = glob->TELStripSizeY[0]/TMath::Sqrt(12.0);
	  size++;
	}

      }//n

      LineGraph = new TGraphErrors(size,PlanePosition,HitPosition,0,HitPositionErr);
      LineGraph->Fit("f1","Q");

      SlopeDigY.push_back(f1->GetParameter(0));
      ConstDigY.push_back(f1->GetParameter(1));
      Chi2DigY.push_back(f1->GetChisquare());

      delete LineGraph;

    }

    delete [] PlanePosition;
    delete [] HitPosition;
    delete [] HitPositionErr;

    while1++;

  }//while1

  //CogX
  while1 = 0;
  while(CogX[0][while1] > 0.0 && while1 < glob->TELMaxHit){

    size = 1;
    FitInfo = false;

    for(Int_t n = 1; n < glob->TELNumberOfPlanes; n++){

      if(CogX[n][while1] > 0.0){
	FitInfo = true;
	size++;
      }

    }//n

    PlanePosition = new Float_t[size];
    HitPosition = new Float_t[size];
    HitPositionErr = new Float_t[size];

    if(FitInfo){

      size = 0;
      for(Int_t n = 0; n < glob->TELNumberOfPlanes; n++){

	if(CogX[n][while1] > 0.0){
	  PlanePosition[size] = glob->TELOffsetZ[n];
	  HitPosition[size] = CogX[n][while1];
	  HitPositionErr[size] = glob->TELSigmaXCogX[n];
	  size++;
	}

      }//n

      LineGraph = new TGraphErrors(size,PlanePosition,HitPosition,0,HitPositionErr);
      LineGraph->Fit("f1","Q");

      SlopeCogX.push_back(f1->GetParameter(0));
      ConstCogX.push_back(f1->GetParameter(1));
      Chi2CogX.push_back(f1->GetChisquare());

      delete LineGraph;

    }

    delete [] PlanePosition;
    delete [] HitPosition;
    delete [] HitPositionErr;

    while1++;

  }//while1

  //CogY
  while1 = 0;
  while(CogY[0][while1] > 0.0 && while1 < glob->TELMaxHit){

    size = 1;
    FitInfo = false;

    for(Int_t n = 1; n < glob->TELNumberOfPlanes; n++){

      if(CogY[n][while1] > 0.0){
	FitInfo = true;
	size++;
      }

    }//n

    PlanePosition = new Float_t[size];
    HitPosition = new Float_t[size];
    HitPositionErr = new Float_t[size];

    if(FitInfo){

      size = 0;
      for(Int_t n = 0; n < glob->TELNumberOfPlanes; n++){

	if(CogY[n][while1] > 0.0){
	  PlanePosition[size] = glob->TELOffsetZ[n];
	  HitPosition[size] = CogY[n][while1];
	  HitPositionErr[size] = glob->TELSigmaXCogY[n];
	  size++;
	}

      }//n

      LineGraph = new TGraphErrors(size,PlanePosition,HitPosition,0,HitPositionErr);
      LineGraph->Fit("f1","Q");

      SlopeCogY.push_back(f1->GetParameter(0));
      ConstCogY.push_back(f1->GetParameter(1));
      Chi2CogY.push_back(f1->GetChisquare());

      delete LineGraph;

    }

    delete [] PlanePosition;
    delete [] HitPosition;
    delete [] HitPositionErr;

    while1++;

  }//while1

}//FindTrack

void TELTrack::FindHits(Int_t Event){

  Float_t DigX[glob->TELNumberOfPlanes][glob->TELMaxHit];
  Float_t DigY[glob->TELNumberOfPlanes][glob->TELMaxHit];
  Float_t CogX[glob->TELNumberOfPlanes][glob->TELMaxHit];
  Float_t CogY[glob->TELNumberOfPlanes][glob->TELMaxHit];
  
  ntuple->SetBranchAddress("DigX",&DigX[0][0]);
  ntuple->SetBranchAddress("DigY",&DigY[0][0]);
  ntuple->SetBranchAddress("CogX",&CogX[0][0]);
  ntuple->SetBranchAddress("CogY",&CogY[0][0]);

  if(Event >= NumberOfEvents){

    cout << " Error - you have superpassed number of events in run " << endl; 
    return;

  }

  ntuple->GetEntry(Event);

  Float_t PosX[glob->TELNumberOfPlanes];
  Float_t PosY[glob->TELNumberOfPlanes];

  HitDigX.clear();
  HitDigY.clear();
  Chi2Dig.clear();

  HitCogX.clear();
  HitCogY.clear();
  Chi2Cog.clear();

  Int_t while1;
  Bool_t FitInfo;

  //Dig  
  //resolution of telescope planes
  _planeResolution[0] =  glob->TELStripSizeX[0]/TMath::Sqrt(12.0);

  for(Int_t n = 2; n < _nTelPlanes; n++){
    _planeResolution[n] =  glob->TELStripSizeX[n-1]/TMath::Sqrt(12.0);
  }//n

  AFZMatrix();

  while1 = 0;
  while(DigX[0][while1] > 0.0 && DigY[0][while1] > 0.0 && while1 < glob->TELMaxHit){

    FitInfo = true;

    PosX[0] = DigX[0][while1];
    PosY[0] = DigY[0][while1];

    for(Int_t n = 1; n < glob->TELNumberOfPlanes; n++){
      
      if(DigX[n][while1] < 0.0 || DigY[n][while1] < 0.0){
	FitInfo = false;
	continue;
      }

      PosX[n] = DigX[n][while1];
      PosY[n] = DigY[n][while1];

    }//n    

    if(FitInfo){

      Chi2Dig.push_back(AFZFit(PosX,PosY));
      HitDigX.push_back(_fitX[1]);
      HitDigY.push_back(_fitY[1]);

    }//if

    while1++;

  }//while1

  //Cog
  //resolution of telescope planes
  _planeResolution[0] = glob->TELSigmaXCogX[0];

  for(Int_t n = 2; n < _nTelPlanes; n++){
    _planeResolution[n] = glob->TELSigmaXCogX[n-1];
  }//n

  AFZMatrix();

  while1 = 0;
  while(CogX[0][while1] > 0.0 && CogY[0][while1] > 0.0 && while1 < glob->TELMaxHit){

    FitInfo = true;

    PosX[0] = CogX[0][while1];
    PosY[0] = CogY[0][while1];

    for(Int_t n = 1; n < glob->TELNumberOfPlanes; n++){
      
      if(DigX[n][while1] < 0.0 || DigY[n][while1] < 0.0){
	FitInfo = false;
	continue;
      }

      PosX[n] = CogX[n][while1];
      PosY[n] = CogY[n][while1];

    }//n    

    if(FitInfo){

      Chi2Cog.push_back(AFZFit(PosX,PosY));
      HitCogX.push_back(_fitX[1]);
      HitCogY.push_back(_fitY[1]);

    }//if

    while1++;

  }//while1
  
}//FindHits

void TELTrack::GetHits(Int_t Event){

  Float_t CogX[glob->TELNumberOfPlanes][glob->TELMaxHit];
  Float_t CogY[glob->TELNumberOfPlanes][glob->TELMaxHit];
  
  ntuple->SetBranchAddress("CogX",&CogX[0][0]);
  ntuple->SetBranchAddress("CogY",&CogY[0][0]);

  if(Event >= NumberOfEvents){

    cout << " Error - you have superpassed number of events in run " << endl; 
    return;

  }

  ntuple->GetEntry(Event);
  
  Float_t PosX[glob->TELNumberOfPlanes];
  Float_t PosY[glob->TELNumberOfPlanes];

  HitCogX.clear();
  HitCogY.clear();
  Chi2Cog.clear();

  Int_t while1;
  Bool_t FitInfo;

  //Cog
  //resolution of telescope planes
  _planeResolution[0] = glob->TELSigmaXCogX[0];

  for(Int_t n = 2; n < _nTelPlanes; n++){
    _planeResolution[n] = glob->TELSigmaXCogX[n-1];
  }//n

  AFZMatrix();

  while1 = 0;
  while(CogX[0][while1] > 0.0 && CogY[0][while1] > 0.0 && while1 < glob->TELMaxHit){

    FitInfo = true;

    PosX[0] = CogX[0][while1];
    PosY[0] = CogY[0][while1];

    for(Int_t n = 1; n < glob->TELNumberOfPlanes; n++){
      
      if(CogX[n][while1] < 0.0 || CogY[n][while1] < 0.0){
	FitInfo = false;
	continue;
      }

      PosX[n] = CogX[n][while1];
      PosY[n] = CogY[n][while1];

    }//n    

    if(FitInfo){

      Chi2Cog.push_back(AFZFit(PosX,PosY));
      HitCogX.push_back(_fitX[1]);
      HitCogY.push_back(_fitY[1]);

    }//if

    while1++;

  }//while1

}//GetHits

Int_t TELTrack::DoAnalFit(Double_t * pos, Double_t *err)
{ 
 
  for(int ipl=0; ipl<_nTelPlanes;ipl++)
    {
      if(_isActive[ipl] && err[ipl]>0)
        err[ipl]=1./err[ipl]/err[ipl] ;
      else
        err[ipl] = 0. ;
      
      pos[ipl]*=err[ipl];
    }

  
  for(int ipl=0; ipl<_nTelPlanes;ipl++)
    for(int jpl=0; jpl<_nTelPlanes;jpl++)
      {
	int imx=ipl+jpl*_nTelPlanes;

	_fitArray[imx] = 0.;

	if(jpl==ipl-2)
	  _fitArray[imx] += _planeDist[ipl-2]*_planeDist[ipl-1]*_planeScat[ipl-1] ;

	if(jpl==ipl+2)
	  _fitArray[imx] += _planeDist[ipl]*_planeDist[ipl+1]*_planeScat[ipl+1] ;

	if(jpl==ipl-1)
	  {
	    if(ipl>0 &&  ipl < _nTelPlanes-1)
              _fitArray[imx] -= _planeDist[ipl-1]*(_planeDist[ipl]+_planeDist[ipl-1])*_planeScat[ipl] ;
	    if(ipl>1)
              _fitArray[imx] -= _planeDist[ipl-1]*(_planeDist[ipl-1]+_planeDist[ipl-2])*_planeScat[ipl-1] ;
	  }

	if(jpl==ipl+1)
	  {
	    if(ipl>0 && ipl < _nTelPlanes-1)
	      _fitArray[imx] -= _planeDist[ipl]*(_planeDist[ipl]+_planeDist[ipl-1])*_planeScat[ipl] ;
	    if(ipl < _nTelPlanes-2)
	      _fitArray[imx] -= _planeDist[ipl]*(_planeDist[ipl+1]+_planeDist[ipl])*_planeScat[ipl+1] ;
	  }

	if(jpl==ipl)
	  {
            _fitArray[imx] += err[ipl] ;

            if(ipl>0 && ipl<_nTelPlanes-1)
	      _fitArray[imx] += _planeScat[ipl]*(_planeDist[ipl]+_planeDist[ipl-1])*(_planeDist[ipl]+_planeDist[ipl-1]) ;

            if(ipl > 1 )
	      _fitArray[imx] += _planeScat[ipl-1]*_planeDist[ipl-1]*_planeDist[ipl-1] ;

            if(ipl < _nTelPlanes-2)
	      _fitArray[imx] += _planeScat[ipl+1]*_planeDist[ipl]*_planeDist[ipl] ;
	  }

	// For beam constraint

	if(ipl==jpl && ipl<2 && _useBeamConstraint)
	  _fitArray[imx] += _planeScat[0]*_planeDist[1]*_planeDist[1] ;

	if(ipl+jpl==1 && _useBeamConstraint) 
	  _fitArray[imx] -= _planeScat[0]*_planeDist[1]*_planeDist[1] ;


      }

  int status=GaussjSolve(_fitArray,pos,_nTelPlanes) ;

  if(status)
    {
      cerr << "Singular matrix in track fitting algorithm ! " << endl;
      for(int ipl=0;ipl<_nTelPlanes;ipl++)
	err[ipl]=0. ;
    }
  else
    for(int ipl=0;ipl<_nTelPlanes;ipl++)
      err[ipl]=sqrt(_fitArray[ipl+ipl*_nTelPlanes]);

  return status ;

}//DoAnalFit

Double_t TELTrack::GetFitChi2()
{
  double chi2=0. ;

  // Measurements

  for(int ipl=0; ipl<_nTelPlanes;ipl++)
    if(_isActive[ipl])
      {
	if(_planeEx[ipl]>0.)
	  chi2+=(_fitX[ipl]-_planeX[ipl])*(_fitX[ipl]-_planeX[ipl])/_planeEx[ipl]/_planeEx[ipl] ;

	if(_planeEy[ipl]>0.)
	  chi2+=(_fitY[ipl]-_planeY[ipl])*(_fitY[ipl]-_planeY[ipl])/_planeEy[ipl]/_planeEy[ipl] ;

      }

  // Scattering angles
  // Use approximate formulas, corresponding to the approximation 
  // used in fitting algorithm

  for(int ipl=1; ipl<_nTelPlanes-1;ipl++)
    {
      double th1,th2,dth;
    
      th2=(_fitX[ipl+1]-_fitX[ipl])*_planeDist[ipl] ;
      th1=(_fitX[ipl]-_fitX[ipl-1])*_planeDist[ipl-1] ;
      //    dth=atan(th2)-atan(th1) ;
      dth= th2 - th1 ;
      chi2 += _planeScat[ipl] * dth * dth;

      th2=(_fitY[ipl+1]-_fitY[ipl])*_planeDist[ipl] ;
      th1=(_fitY[ipl]-_fitY[ipl-1])*_planeDist[ipl-1] ;
      //    dth=atan(th2)-atan(th1) ;
      dth= th2 - th1 ;
      chi2 += _planeScat[ipl] * dth * dth;
    }

  // Beam constraint

  if(_useBeamConstraint)
    {
      double dth;
    
      //    dth=atan((_fitX[1]-_fitX[0])*_planeDist[0]) ;
      dth=(_fitX[1]-_fitX[0])*_planeDist[0] ;
      chi2 += _planeScat[0] * dth * dth;

      //    dth=atan((_fitY[1]-_fitY[0])*_planeDist[0]) ;
      dth=(_fitY[1]-_fitY[0])*_planeDist[0] ;
      chi2 += _planeScat[0] * dth * dth;
    }

  return chi2 ;
}//GetFitChi2

Int_t TELTrack::GaussjSolve(Double_t *alfa,Double_t *beta,int n)
{
  int *ipiv;
  int *indxr;
  int *indxc;
  int i,j,k;
  int irow=0;
  int icol=0;
  double abs,big,help,pivinv;

  ipiv = new int[n];
  indxr = new int[n];
  indxc = new int[n];

  for(i=0;i<n;ipiv[i++]=0);

  for(i=0;i<n;i++)
    {
      big=0.;
      for(j=0;j<n;j++)
	{
	  if(ipiv[j]==1)continue;
	  for(k=0;k<n;k++)
	    {
	      if(ipiv[k]!=0)continue;
	      abs=fabs(alfa[n*j+k]);
	      if(abs>big)
		{
		  big=abs;
		  irow=j;
		  icol=k;
		}
	    }
	}
      ipiv[icol]++;

      if(ipiv[icol]>1)
	return 1;

      if(irow!=icol)
	{
	  help=beta[irow];
	  beta[irow]=beta[icol];
	  beta[icol]=help;
	  for(j=0;j<n;j++)
	    {
	      help=alfa[n*irow+j];
	      alfa[n*irow+j]=alfa[n*icol+j];
	      alfa[n*icol+j]=help;
	    }
	}
      indxr[i]=irow;
      indxc[i]=icol;

      if(alfa[n*icol+icol]==0.)
	return 1;

      help=alfa[n*icol+icol];
      pivinv=1./help;
      alfa[n*icol+icol]=1.;
      for(j=0;j<n;alfa[n*icol+(j++)]*=pivinv);
      beta[icol]*=pivinv;

      for(j=0;j<n;j++)
	{
	  if(j==icol)continue;
	  help=alfa[n*j+icol];
	  alfa[n*j+icol]=0.;
	  for(k=0;k<n;k++)
	    alfa[n*j+k]-=alfa[n*icol+k]*help;
	  beta[j]-=beta[icol]*help;
	}
    }

  for(i=n-1;i>=0;i--)
    {
      if(indxr[i]==indxc[i])continue;
      for(j=0;j<n;j++)
	{
	  help=alfa[n*j+indxr[i]];
	  alfa[n*j+indxr[i]]=alfa[n*j+indxc[i]];
	  alfa[n*j+indxc[i]]=help;
	}
    }

  delete [] ipiv;
  delete [] indxr;
  delete [] indxc;

  return 0;
}//GaussSolve

void TELTrack::AFZMatrix(){
  
  Float_t MAPSTheta = TMath::ACos(TMath::Cos(glob->DUTPhiX)*TMath::Cos(glob->DUTPhiY));
  _planeThickness[1] = glob->DUTThickness/TMath::Cos(MAPSTheta);

  // Fill nominal fit matrices and 
  // calculate expected precision of track fitting
  
  // Planes have to be ordered in position along the beam line !
  // This is not checked !!!

  _planePosition[1] = glob->DUTOffsetZ;
  
  for(int ipl=0; ipl<_nTelPlanes ; ipl++)
    {
      if(ipl>0)
	_planeDist[ipl-1]=1./(_planePosition[ipl] - _planePosition[ipl-1]) ;
      
      _planeScat[ipl]= 0.0136/_eBeam * sqrt(_planeThickness[ipl]/_planeX0[ipl]) * (1.+0.038*TMath::Log(_planeThickness[ipl]/_planeX0[ipl])) ;
      
      if(ipl==0 && _useBeamConstraint)
	_planeScat[ipl]= 1./(_planeScat[ipl]*_planeScat[ipl]+ _beamSpread*_beamSpread) ; 
      else
	_planeScat[ipl]= 1./(_planeScat[ipl] * _planeScat[ipl]) ; 
      
      _fitX[ipl] =_fitY[ipl] = 0. ;
      _nominalError[ipl]= _planeResolution[ipl];
    }

  // Fit with nominal parameters
  int status = DoAnalFit(_fitX,_nominalError);
  cerr << "track fit" << endl;
  if(status)
    //cerr << "Event " << ipl << ":" << endl;
    cerr << "\n Fit with nominal geometry failed !?!" << endl ;

  // Store fit matrix

  for(int imx=0; imx < arrayDim; imx++) 
    _nominalFitArray[imx] = _fitArray[imx];

}

Double_t TELTrack::NominalFit()
{
  for(int ipl=0; ipl<_nTelPlanes;ipl++)
    {
      _fitEy[ipl]=_fitEx[ipl]=_nominalError[ipl];
      
      _fitX[ipl]=0. ;
      _fitY[ipl]=0. ;
      for(int jpl=0; jpl<_nTelPlanes;jpl++)
	{
	  if(_planeEx[jpl]>0.)
	    _fitX[ipl]+=_nominalFitArray[ipl+jpl*_nTelPlanes]*_planeX[jpl]/_planeEx[jpl]/_planeEx[jpl];
	  if(_planeEy[jpl]>0.)
	    _fitY[ipl]+=_nominalFitArray[ipl+jpl*_nTelPlanes]*_planeY[jpl]/_planeEy[jpl]/_planeEy[jpl];
	}
    }

  Double_t chi2 = GetFitChi2();

  return chi2 ;
}

Double_t TELTrack::AFZFit(Float_t PosX[], Float_t PosY[]){
  
  Double_t Chi2;

  _planeX[0] = PosX[0];
  _planeX[1] = 0.0;
  _planeEx[0] = _planeResolution[0];
  _planeEx[1] = 0.0;

  _planeY[0] = PosY[0];
  _planeY[1] = 0.0;
  _planeEy[0] = _planeResolution[0];
  _planeEy[1] = 0.0;

  for(Int_t n = 2; n < _nTelPlanes; n++){

    _planeX[n] = PosX[n-1];
    _planeEx[n] = _planeResolution[n];
    
    _planeY[n] = PosY[n-1];
    _planeEy[n] = _planeResolution[n];

  }//n

  _planeX[2] = PosX[1];
  _planeX[3] = PosX[2];
  _planeEx[2] = _planeResolution[2];
  _planeEx[3] = _planeResolution[3];

  _planeY[2] = PosY[1];
  _planeY[3] = PosY[2];
  _planeEy[2] = _planeResolution[2];
  _planeEy[3] = _planeResolution[3];

  Chi2 = NominalFit();

  /*
  cout << "Chi2 " << Chi2 << endl; 
 
  cout << "X" << endl;
  cout << PosX[0] << " " << _fitX[0] << endl;
  cout << PosX[1] << " " << _fitX[2] << endl;
  cout << PosX[2] << " " << _fitX[3] << endl;
  cout << "MAPS X" << endl;
  cout << _fitX[1] << endl << endl;

  cout << "Y" << endl;
  cout << PosY[0] << " " << _fitY[0] << endl;
  cout << PosX[1] << " " << _fitY[2] << endl;
  cout << PosX[2] << " " << _fitY[3] << endl;
  cout << "MAPS Y" << endl;
  cout << _fitY[1] << endl << endl;
  */

  return Chi2; 

}

void TELTrack::PlotTracks(){

  //Track Parameters Dig
  TCanvas* TELTrackcanv11 = new TCanvas("TELTrackc1","",1200,800);
  TELTrackcanv11->SetFillColor(0);
  TELTrackcanv11->Divide(3,2);

  TH1F* x_a_Dig = new TH1F("slopexDig","x slope",200,-0.05,0.05);
  TH1F* x_b_Dig = new TH1F("bxDig","b constant for x track",100,0,glob->TELNumberOfStripsX[0] * glob->TELStripSizeX[0]);
  TH1F* y_a_Dig = new TH1F("slopeyDig","y slope",200,-0.05,0.05);
  TH1F* y_b_Dig = new TH1F("byDig","b constant for y track",100,0,glob->TELNumberOfStripsY[0] * glob->TELStripSizeY[0]);
  TH1F* x_chi2_Dig = new TH1F("x_chi2Dig","#chi^{2} x",100,-1,100);
  TH1F* y_chi2_Dig = new TH1F("y_chi2Dig","#chi^{2} y",100,-1,100);

  //Track Parameters Cog
  TCanvas* TELTrackcanv12 = new TCanvas("TELTrackc2","",1200,800);
  TELTrackcanv12->SetFillColor(0);
  TELTrackcanv12->Divide(3,2);

  TH1F* x_a_Cog = new TH1F("slopexCog","x slope",200,-0.05,0.05);
  TH1F* x_b_Cog = new TH1F("bxCog","b constant for x track",100,0,glob->TELNumberOfStripsX[0] * glob->TELStripSizeX[0]);
  TH1F* y_a_Cog = new TH1F("slopeyCog","y slope",200,-0.05,0.05);
  TH1F* y_b_Cog = new TH1F("byCog","b constant for y track",100,0,glob->TELNumberOfStripsY[0] * glob->TELStripSizeY[0]);
  TH1F* x_chi2_Cog = new TH1F("x_chi2Cog","#chi^{2} x",100,-1,100);
  TH1F* y_chi2_Cog = new TH1F("y_chi2Cog","#chi^{2} y",100,-1,100);

  TCanvas* TELTrackcanv13 = new TCanvas("TELTrackc3","",1200,600);
  TELTrackcanv13->SetFillColor(0);
  TELTrackcanv13->Divide(2);

  TCanvas* TELTrackcanv14 = new TCanvas("TELTrackc4","",600,600);
  TELTrackcanv14->SetFillColor(0);

  TH1F* Chi2AFZDig = new TH1F("Chi2AFZDig","#chi^{2} Dig",200,0,100);
  TH1F* Chi2AFZCog = new TH1F("Chi2AFZCog","#chi^{2} Cog",200,0,100);

  //TF1* Chi2Fun1 = new TF1("Chi2Fun1","[0]*x^([1]/2.0 - 1.0)*exp(-x/2.0)",0,20);
  //TF1* Chi2Fun2 = new TF1("Chi2Fun2","[0]*x^([1]/2.0 - 1.0)*exp(-x/2.0)",0,20);
  TF1* Chi2Fun1 = new TF1("Chi2Fun1","[0]*exp(-[1]*x)",0,30);
  TF1* Chi2Fun2 = new TF1("Chi2Fun2","[0]*exp(-[1]*x)",0,30);

  for(Int_t i = 0; i < NumberOfEvents; i++){

    FindTracks(i);

    for(Int_t n = 0; n < SlopeDigX.size(); n++){
      x_a_Dig->Fill(SlopeDigX[n]);
      x_b_Dig->Fill(ConstDigX[n]);
      x_chi2_Dig->Fill(Chi2DigX[n]);
    }

    for(Int_t n = 0; n < SlopeDigY.size(); n++){
      y_a_Dig->Fill(SlopeDigY[n]);
      y_b_Dig->Fill(ConstDigY[n]);
      y_chi2_Dig->Fill(Chi2DigY[n]);
    }

    for(Int_t n = 0; n < SlopeCogX.size(); n++){
      x_a_Cog->Fill(SlopeCogX[n]);
      x_b_Cog->Fill(ConstCogX[n]);
      x_chi2_Cog->Fill(Chi2CogX[n]);
    }

    for(Int_t n = 0; n < SlopeCogY.size(); n++){
      y_a_Cog->Fill(SlopeCogY[n]);
      y_b_Cog->Fill(ConstCogY[n]);
      y_chi2_Cog->Fill(Chi2CogY[n]);
    }

    FindHits(i);

    for(Int_t n = 0; n < Chi2Dig.size(); n++) Chi2AFZDig->Fill(Chi2Dig[n]);
    for(Int_t n = 0; n < Chi2Cog.size(); n++) Chi2AFZCog->Fill(Chi2Cog[n]);
    
  }//i

  //Cog
  //resolution of telescope planes
  _planeResolution[0] = glob->TELSigmaXCogX[0];

  for(Int_t n = 2; n < _nTelPlanes; n++){
    _planeResolution[n] = glob->TELSigmaXCogX[n-1];
  }//n

  AFZMatrix();

  cout << "**********************************************" << endl;
  cout << "Error of the position reconstruction in:" << endl;
  for(int ipl=0; ipl<_nTelPlanes ; ipl++){
    cout << "plane nr " << ipl << " - " << _nominalError[ipl] << endl;
  }
  cout << "**********************************************" << endl;

  TELTrackcanv11->cd(1);
  x_a_Dig->Draw();
  TELTrackcanv11->cd(2);
  x_b_Dig->Draw();
  TELTrackcanv11->cd(3);
  x_chi2_Dig->Draw();

  TELTrackcanv11->cd(4);
  y_a_Dig->Draw();
  TELTrackcanv11->cd(5);
  y_b_Dig->Draw();
  TELTrackcanv11->cd(6);
  y_chi2_Dig->Draw();

  TELTrackcanv12->cd(1);
  x_a_Cog->Draw();
  TELTrackcanv12->cd(2);
  x_b_Cog->Draw();
  TELTrackcanv12->cd(3);
  x_chi2_Cog->Draw();

  TELTrackcanv12->cd(4);
  y_a_Cog->Draw();
  TELTrackcanv12->cd(5);
  y_b_Cog->Draw();
  TELTrackcanv12->cd(6);
  y_chi2_Cog->Draw();

  TELTrackcanv13->cd(1);
  Chi2AFZDig->Draw();
  Chi2Fun2->SetLineColor(3);
  Chi2AFZDig->Fit("Chi2Fun2");
  TELTrackcanv13->cd(2);
  Chi2AFZCog->Draw();
  Chi2Fun1->SetLineColor(5);
  Chi2AFZCog->Fit("Chi2Fun1");

  TELTrackcanv14->cd();
  Chi2AFZDig->SetLineColor(4);
  Chi2AFZDig->SetLineWidth(2);
  Chi2AFZDig->Draw();
  Chi2AFZCog->SetLineColor(2);
  Chi2AFZCog->SetLineWidth(2);
  Chi2AFZCog->Draw("SAME");


}//PlotTracks

TELTrack::~TELTrack(){

  // Clean memory 

  delete [] _planeSort ;
  delete [] _planeID ;
  delete [] _planeShiftX ;
  delete [] _planeShiftY ;
  delete [] _planeRotZ ;
  delete [] _planePosition ;
  delete [] _planeThickness  ;
  delete [] _planeX0  ;
  delete [] _planeResolution ;
  delete [] _planeDist ;
  delete [] _planeScat ;
  delete [] _isActive ;
   
  delete [] _planeX ;
  delete [] _planeEx ;
  delete [] _planeY ;
  delete [] _planeEy ;
   
  delete [] _fitX  ;
  delete [] _fitEx ;
  delete [] _fitY ;
  delete [] _fitEy ;
  delete [] _fitArray ;
   
  delete [] _nominalFitArray ;
  delete [] _nominalError ;

  delete [] Path;
  //ff->Close();

}
